using System.Collections;
using System.Collections.Generic;
using UnityEngine;

public class Pid : MonoBehaviour
{
    public float kp;
    public float ki;
    public float kd;
    public float max_acc = 65535;
    private float acc=0;
    private float last_error = 0;
    public bool flag = true;
    public float Control(float error)
    {
        float res;
        //if (error>1)
        //{
        //    error = 1;
        //}else if (error < -1)
        //{
        //    error = -1;
        //}
        
        acc += error;
        if (acc > max_acc)
        {
            acc = max_acc;
        }
        else if (acc < -max_acc)
        {
            acc = -max_acc;
        }
        res =kp*error+ki*acc-kd*(error-last_error);
        last_error = error;
        if (flag)
        {
            return res;
        }
        else
        {
            acc = 0;
            return 0;
        }
       
    }
    public static Pid AddPid(GameObject target,float kp,float ki,float kd)
    {
        var res=target.AddComponent<Pid>();
        res.kp = kp;
        res.ki = ki;
        res.kd = kd;
        return res;
    }
    public static Pid AddPid(GameObject target, Vector3 k)
    {
      return AddPid(target,k.x,k.y,k.z);
    }
}
